

// /f/bIII52III201III152IIIMoveJ(-116.061,-90.725,91.261,-90.757,-90.399,2.142,100,498.776,474.670,-179.764,-0.390,-28.204,0,0,100,180,100,0.000,0.000,0.000,0.000,0,0,0,0,0,0,0,0)III/b/f

use std::sync::Arc;

use serde::Serialize;
use tokio::io::AsyncReadExt;

use crate::{error::AppErr, utils::new_buf};

use super::cmd::{movej::MoveJ, movel::MoveL};



pub fn make_proto(cnt: u16, cmd_id: u32, data: &str) -> String {
    format!("/f/bIII{}III{}III{}III{}III/b/f", cnt, cmd_id, data.len(), data)
}

pub async fn read_proto<R: AsyncReadExt + Unpin>(r: &mut R) -> Result<String, AppErr> {
    let mut buf = new_buf(256);
    let mut s = String::new();

    while !s.ends_with("/b/f") {
        let n = r.read(&mut buf).await?;
        if n == 0 {
            return Err(AppErr::RobotClosed);
        }
        s.push_str( unsafe { std::str::from_utf8_unchecked(&buf[..n]) } );
    }

    Ok(s)
}

pub async fn read_status<R: AsyncReadExt + Unpin>(r: &mut R) -> Result<Arc<RobotStatus>, AppErr> {
    const HEAD: u16 = 0x5A5A;
    let mut head_buf = [0u8; 5];
    r.read_exact(&mut head_buf).await?;

    let head = u16::from_le_bytes([head_buf[0], head_buf[1]]);
    let len = u16::from_le_bytes([head_buf[3], head_buf[4]]) as usize;

    if head != HEAD {
        return Err(AppErr::Static("机械臂状态头错误"));
    }
    let mut buf = new_buf(len + 2);
    r.read_exact(&mut buf).await?;

    let sum = check_sum(&head_buf).wrapping_add( check_sum(&buf[..len]) );
    let sum2 = u16::from_le_bytes([buf[len], buf[len - 1]]);
    if sum != sum2 {
        return Err(AppErr::Static("机械臂状态校验和错误"));
    }

    let status = RobotStatus::new(&buf)?;

    Ok(status)
}


#[derive(Debug, Serialize, Default)]
pub struct RobotStatus {
    program_state: u8, // 1-停止；2-运行；3-暂停；4-拖动
    error_code: u8,
    robot_mode: u8, // 0-自动模式，1-手动模式；2-拖动模式
    jt_cur_pos: [f64; 6],
    tl_cur_pos: [f64; 6],
    tool_num: i32,
    jt_cur_tor: [f64; 6],
    program_name: [u8; 20],
    prog_total_line: u8,
    prog_cur_line: u8,
    cl_dgt_output_h: u8,
    cl_dgt_output_l: u8,
    tl_dgt_output_l: u8,
    cl_dgt_input_h: u8,
    cl_dgt_input_l: u8,
    tl_dgt_input_l: u8,
    ft_data: [f64; 6],
    ft_act_status: u8,
    emergency_stop: u8,
    robot_motion_done: i32,
    gripper_motion_done: u8,
}

impl RobotStatus {

    pub fn movej(&self, speed: f32, acc: f32, bland_t: f32) -> MoveJ {
        MoveJ::new(self.jt_cur_pos, self.tl_cur_pos, speed, acc, bland_t)
    }

    pub fn movel(&self, speed: f32, acc: f32, bland_r: f32) -> MoveL {
        MoveL::new(self.jt_cur_pos, self.tl_cur_pos, speed, acc, bland_r)
    }

    pub fn is_robot_done(&self) -> bool {
        (self.robot_motion_done & 0x01) != 0
    }

    pub fn is_jaw_done(&self) -> bool {
        self.gripper_motion_done != 0
    }

    pub fn is_stop(&self) -> bool {
        self.emergency_stop == 1
    }

    pub fn jaw_msg(&self) -> &'static str {
        match self.gripper_motion_done {
            0 => "运动未完成",
            1 => "夹爪停止(未夹到东西)",
            2 => "夹爪停止(夹到东西)",
            3 => "夹到物体后掉落",
            _ => "夹爪状态未知",
        }
    }

    pub fn err_msg(&self) -> &'static str {
        match self.error_code {
            0 => "正常",
            1 => "驱动器故障",
            2 => "超出软限位故障",
            3 => "碰撞故障",
            4 => "奇异位姿",
            5 => "从站错误",
            6 => "指令点错误",
            7 => "IO错误",
            8 => "夹爪错误",
            9 => "文件错误",
            10 => "参数错误",
            11 => "扩展轴超出软限位错误",
            12 => "关节配置警告",
            _ => "未知错误",
        }
    }

    pub fn new(buf: &[u8]) -> Result<Arc<Self>, AppErr> {
        if buf.len() < 234 {
            return Err(AppErr::Static("机械臂状态字节数错误"));
        }
        let status = Arc::new(RobotStatus {
            program_state: decode_u8(buf),
            error_code: decode_u8(&buf[1..]),
            robot_mode: decode_u8(&buf[2..]),
            jt_cur_pos: decode_f64_8(&buf[3..]),
            tl_cur_pos: decode_f64_8(&buf[(51)..]),
            tool_num: decode_i32(&buf[(99)..]),
            jt_cur_tor: decode_f64_8(&buf[(103)..]),
            program_name: decode_u20(&buf[151..]),
            prog_total_line: decode_u8(&buf[171..]),
            prog_cur_line: decode_u8(&buf[172..]),
            cl_dgt_output_h: decode_u8(&buf[173..]),
            cl_dgt_output_l: decode_u8(&buf[174..]),
            tl_dgt_output_l: decode_u8(&buf[175..]),
            cl_dgt_input_h: decode_u8(&buf[176..]),
            cl_dgt_input_l: decode_u8(&buf[177..]),
            tl_dgt_input_l: decode_u8(&buf[178..]),
            ft_data: decode_f64_8(&buf[179..]),
            ft_act_status: decode_u8(&buf[227..]),
            emergency_stop: decode_u8(&buf[228..]),
            robot_motion_done: decode_i32(&buf[229..]),
            gripper_motion_done: decode_u8(&buf[233..])
        });

        Ok(status)
    }
}

const fn decode_u8(buf: &[u8]) -> u8 {
    buf[0]
}

const fn decode_i32(buf: &[u8]) -> i32 {
    (buf[0] as i32) |
    ((buf[1] as i32) << 8) |
    ((buf[2] as i32) << 16) |
    ((buf[3] as i32) << 24)
}

const fn decode_f64(buf: &[u8]) -> f64 {
    f64::from_le_bytes([
        buf[0],
        buf[1],
        buf[2],
        buf[3],
        buf[4],
        buf[5],
        buf[6],
        buf[7]
    ])
}

const fn decode_u20(buf: &[u8]) -> [u8; 20] {
    [
        buf[0], buf[1], buf[2], buf[3], buf[4],
        buf[5], buf[6], buf[7], buf[8], buf[9],
        buf[10], buf[11], buf[12], buf[13], buf[14],
        buf[15], buf[16], buf[17], buf[18], buf[19],
    ]
}

#[inline(always)]
fn decode_f64_8(buf: &[u8]) -> [f64; 6] {
    [
        decode_f64(buf),
        decode_f64(&buf[8..]),
        decode_f64(&buf[16..]),
        decode_f64(&buf[24..]),
        decode_f64(&buf[32..]),
        decode_f64(&buf[40..]),
    ]
}

fn check_sum(buf: &[u8]) -> u16 {
    let mut s: u16 = 0;
    for v in buf {
        s = s.wrapping_add(*v as u16);
    }
    s
}




